#include "nanocalib.h"
#include "ui_nanocalib.h"

#define xlim1 50
#define xlim2 590
#define ylim1 50
#define ylim2 430

extern CvMat *position_points;

nanoCalib::nanoCalib(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::nanoCalib)
{
    ui->setupUi(this);
    this->setWindowTitle("Control window");

    /** Initializing Space Explorer    */
    joystick = new Joystick();

    joystick->open();
    joystickTimer = new QTimer();
    joystickTimer->setInterval(1);

    // connecting joystick timer to joystick polling function
    connect(joystickTimer, SIGNAL(timeout()), joystick, SLOT(pollDevice()));
    joystickTimer->start();

    /** Initializing Phantom Omni    */
    omni = new OmniWidget();

    omniTimer = new QTimer();

    // omniTimer->setInterval(100);
    // connect(omniTimer, SIGNAL(timeout()), omni, SLOT(pollDevice()));


    /** Initializing video widget and adding layout     */
    // creating video window. Temperorily disabled capture usingl libdc1394. Current using capture provided by opencv
    // captureThread = new CaptureThread();
    // captureThread->start();
    // captureThread->setPriority(QThread::HighestPriority);
    captureThread = NULL;

    videoWindow = new QWidget();
    videoWindow->setWindowTitle("Video");

    // making layout for video window
    QGridLayout *videoWindowLayout = new QGridLayout;

    // creating videowidget
    cam = new VideoWidget(this,joystick);
    cam->initAxis(xlim1, xlim2, ylim1, ylim2);
    videoWindowLayout->addWidget(cam,0,0,1,12,0);

    // creating windows for start and stop recording in the video window
    recState = new QCheckBox();
    videoWindowLayout->addWidget(recState, 1, 0, Qt::AlignCenter);

    // label for video rec state
    QLabel *recStateLabel = new QLabel();
    recStateLabel->setText("Record video");
    videoWindowLayout->addWidget(recStateLabel, 1, 1, Qt::AlignLeft);

    videoWindow->setLayout(videoWindowLayout);
    videoWindow->show();


    /** Initializing Manipulator control thread      */
    // creating control thread to control mm3a manipulator
    cThread = new ControlThread(this, joystick);
    //cThread->setPriority(QThread::HighestPriority);

    qDebug()<<"Main:Created cThread";

    // setting origin
    //cThread->mm3a->center << x << y << 50.0;
    cThread->mm3a->xlimit[0] = xlim1;
    cThread->mm3a->xlimit[1] = xlim2;
    cThread->mm3a->ylimit[0] = ylim1;
    cThread->mm3a->ylimit[1] = ylim2;

    // creating options window object
    mOpts = new ManipulatorOpts();

    qDebug()<<"Main:Created Opts window";

    // reset button
    //connect(ui.resetButton, SIGNAL(clicked()), cThread, SLOT(reset()));

    // simulate checkbox
    //connect(ui.simulateBox, SIGNAL(clicked()), cThread, SLOT(simulate()));

    /**  Control window layout  */

    QGridLayout *mainWindowLayout = new QGridLayout;
    int row = 3;  // row of grid layout
    int column = 0;  // column of grid layout

    // start button
    QPushButton *startExp = new QPushButton("Start Exp");
    connect(startExp, SIGNAL(clicked()), this, SLOT(startExp()));
    mainWindowLayout->addWidget(startExp, row, column, 1, 1, Qt::AlignCenter);
    column++;

    // stop button
    QPushButton *stopExp = new QPushButton("Stop Exp");
    connect(stopExp, SIGNAL(clicked()), this, SLOT(stopExp()));
    mainWindowLayout->addWidget(stopExp, row, column, 1, 1, Qt::AlignCenter);
    column++;

    // adding sliders for cvThreshold
    QLabel *cvThresholdLabel = new QLabel("cvThreshold");
    mainWindowLayout->addWidget(cvThresholdLabel, row, column, 1, 1, Qt::AlignRight);
    QSlider *cvThresholdSlider = new QSlider(Qt::Horizontal);
    column++;
    connect(cvThresholdSlider, SIGNAL(valueChanged(int)), cam, SLOT(cvThresholdF(int)));
    mainWindowLayout->addWidget(cvThresholdSlider, row,column, 1, 3, Qt::AlignCenter);
    cvThresholdSlider->setValue(cam->cvthreshold);
    cvThresholdSlider->setMaximum(250);

    row++; // 4
    column = 0;
    // reset button
    QPushButton *resetButton = new QPushButton("Reset");
    connect(resetButton, SIGNAL(clicked()), cThread, SLOT(reset()));
    mainWindowLayout->addWidget(resetButton, row, column, 1, 1, Qt::AlignCenter);

    column= column+2;
    // adding sliders for cvThresholdMax
    QLabel *cvThresholdMaxLabel = new QLabel("cvThresholdMax");
    mainWindowLayout->addWidget(cvThresholdMaxLabel, row, column, 1, 1, Qt::AlignRight);
    column++;
    QSlider *cvThresholdMaxSlider = new QSlider(Qt::Horizontal);
    connect(cvThresholdMaxSlider, SIGNAL(valueChanged(int)), cam, SLOT(cvThresholdMaxF(int)));
    mainWindowLayout->addWidget(cvThresholdMaxSlider, row,column, 1, 3, Qt::AlignCenter);
    cvThresholdMaxSlider->setRange(0,255);
    cvThresholdMaxSlider->setValue(cam->cvthresholdMax);

    row++; //5
    column = 0;
    // simulate checkbox and label
    QLabel *simulateLabel = new QLabel("Simulate");
    mainWindowLayout->addWidget(simulateLabel, row, column, 1, 1, Qt::AlignLeft);
    column++;
    QCheckBox *simulateBox = new QCheckBox();
    connect(simulateBox, SIGNAL(clicked()), cThread, SLOT(simulate()));
    mainWindowLayout->addWidget(simulateBox, row, column, 1, 1, Qt::AlignCenter);
    column++;

    // adding sliders for blobThreshold
    QLabel *blobThresholdLabel = new QLabel("blobThreshold");
    mainWindowLayout->addWidget(blobThresholdLabel, row, column, 1, 1, Qt::AlignRight);
    column++;
    QSlider *blobThresholdSlider = new QSlider(Qt::Horizontal);
    connect(blobThresholdSlider, SIGNAL(valueChanged(int)), cam, SLOT(blobThresholdF(int)));
    mainWindowLayout->addWidget(blobThresholdSlider, row, column, 1, 3, Qt::AlignCenter);
    blobThresholdSlider->setValue(cam->blobthreshold);


    row++; // 6
    column = 0;
    QButtonGroup *modeGroup = new QButtonGroup();
    QLabel *expModeLabel = new QLabel("Experiment Mode");
    QLabel *freeModeLabel = new QLabel("Free Mode");
    expMode = new QCheckBox();
    freeMode = new QCheckBox();

    modeGroup->addButton(expMode, 1);
    modeGroup->addButton(freeMode,2);
    modeGroup->exclusive();
    freeMode->setChecked(1);

    mainWindowLayout->addWidget(expModeLabel, row, column, 1, 1, Qt::AlignLeft);
    column++;
    mainWindowLayout->addWidget(expMode, row, column, 1, 1, Qt::AlignCenter);
    column++;
    mainWindowLayout->addWidget(freeModeLabel, row, column, 1, 1, Qt::AlignLeft);
    column++;
    mainWindowLayout->addWidget(freeMode, row, column, 1, 1, Qt::AlignCenter);
    connect(modeGroup, SIGNAL(buttonClicked(int)), this, SLOT(setMode()));


    row++; // 7
    column = 0;
    // adding checkbox to select input device
    QButtonGroup *joystickGroup = new QButtonGroup();
    omniBox = new QCheckBox();
    joystickBox = new QCheckBox();
    QLabel *joystickLabel = new QLabel("Space Explorer");
    QLabel *omniLabel = new QLabel("Phantom Omni");


    joystickGroup->addButton(omniBox,1);
    joystickGroup->addButton(joystickBox,2);
    joystickGroup->exclusive();

    mainWindowLayout->addWidget(omniLabel, row, column,1,1, Qt::AlignLeft);
    column++;
    mainWindowLayout->addWidget(omniBox, row, column,1,1, Qt::AlignCenter);
    column++;
    mainWindowLayout->addWidget(joystickLabel, row, column,1,1, Qt::AlignLeft);
    column++;
    mainWindowLayout->addWidget(joystickBox, row, column,1,1, Qt::AlignCenter);
    joystickBox->setChecked(1);

    row++; // 8
    column = 0;
    QButtonGroup *planeGroup = new QButtonGroup();
    QLabel *xyPlaneLabel = new QLabel("XY");
    QLabel *xzPlaneLabel = new QLabel("XZ");
    xyPlane = new QCheckBox();
    xzPlane = new QCheckBox();

    planeGroup->addButton(xyPlane, 1);
    planeGroup->addButton(xzPlane,2);
    planeGroup->exclusive();
    xyPlane->setChecked(1);

    mainWindowLayout->addWidget(xyPlaneLabel, row, column, 1, 1, Qt::AlignLeft);
    column++;
    mainWindowLayout->addWidget(xyPlane, row, column, 1, 1, Qt::AlignCenter);
    column++;
    mainWindowLayout->addWidget(xzPlaneLabel, row, column, 1, 1, Qt::AlignLeft);
    column++;
    mainWindowLayout->addWidget(xzPlane, row, column, 1, 1, Qt::AlignCenter);
    connect(planeGroup, SIGNAL(buttonClicked(int)), this, SLOT(setExpPlane()));

    row++; // 9
    column = 0;
    QLabel *blobDetectLabel = new QLabel("Blob detection");
    QCheckBox *blobDetectBox = new QCheckBox();

    mainWindowLayout->addWidget(blobDetectLabel, row, column, 1,1, Qt::AlignLeft);
    column++;
    mainWindowLayout->addWidget(blobDetectBox, row, column, 1,1, Qt::AlignCenter);
    connect(blobDetectBox, SIGNAL(stateChanged(int)), this, SLOT(setBlobDetect(int)));


    /**  Dropdown box for listing different step size models that can be used
         * @brief comboBox
         */
    selectModel = new QComboBox();
    selectModel->addItem(tr("Nominal model"));
    selectModel->addItem(tr("True model"));

    QLabel *modelLabel = new QLabel("Select model");

    row = 6;
    column = 3;
    column++;
    column++;
    mainWindowLayout->addWidget(modelLabel, row, column, 1, 1, Qt::AlignCenter);
    column++;
    mainWindowLayout->addWidget(selectModel, row, column, 1, 1, Qt::AlignCenter);

    /**  Dropdown box for listing operator modes */
    operatorMode = new QComboBox();
    operatorMode->addItem(tr("Blind"));
    operatorMode->addItem(tr("Redirecting"));

    QLabel *operatorLabel = new QLabel("Operator mode");

    row = 7;
    column=column - 2;
    column++;
    mainWindowLayout->addWidget(operatorLabel, row, column, 1, 1, Qt::AlignCenter);
    column++;
    mainWindowLayout->addWidget(operatorMode, row, column, 1, 1, Qt::AlignCenter);

    ui->centralWidget->setLayout(mainWindowLayout);
    connect(this, SIGNAL(destroyed()), this, SLOT(close()));



    /** Dropdown menu for selecting joint update mode. */
    jointUpdateMode = new QComboBox();
    jointUpdateMode->addItem(tr("Update joints"));
    jointUpdateMode->addItem(tr("Lock joints"));

    QLabel *jointUpdateLabel = new QLabel("Joint update mode");

    row = 8;
    column = 5;
    mainWindowLayout->addWidget(jointUpdateLabel, row, column, 1, 1, Qt::AlignCenter);
    column++;
    mainWindowLayout->addWidget(jointUpdateMode, row, column, 1, 1, Qt::AlignCenter);


    // detect change in state of dropdown boxes for model, operator, and joint update mode
    connect(selectModel, SIGNAL(currentIndexChanged(int)), cThread->mm3a, SLOT(setModel(int)));
    connect(operatorMode, SIGNAL(currentIndexChanged(int)), cThread->mm3a, SLOT(setOperatorMode(int)));
    connect(jointUpdateMode, SIGNAL(currentIndexChanged(int)), cThread->mm3a, SLOT(setJointUpdateMode(int)));



    // Set window parameters like position and size
    //    this->setGeometry(QRect(0, 600, 1000, 400 ));
    this->move(QPoint(0, 600));
    videoWindow->move(QPoint(0, 30));
}

nanoCalib::~nanoCalib()
{

    qDebug()<<"\nnanoCalib:: Closing program";
    //    if(captureThread!=NULL && captureThread->isRunning())
    //        captureThread->close();
    //    qDebug()<<"nanoCalib:: Capture thread closed";

    if(omni->isRunning())
        omni->close();
    qDebug()<<"nanoCalib:: Omni thread closed";

    if(cThread->isRunning())
        cThread->close();
    qDebug()<<"nanoCalib:: Control thread closed";

    delete cam;
    qDebug()<<"nanoCalib:: VideoWidget closed";
    delete ui;
}

void nanoCalib::close() {
    qDebug()<< "Closing widget";
    delete cam;
    delete ui;
}

void nanoCalib::startExp() {

    // messagebox to display configuration of the system.
    QMessageBox msgBox;
    QString controlMode;
    qDebug()<< cThread->mode<< cThread->mm3a->getModelMode()<<cThread->mm3a->getOperatorMode()<< cThread->spaceExp;
    if(cThread->mode==0) {         controlMode ="Free";      } else {   controlMode="Experiment";    }

    QString modelMode;
    if(cThread->mm3a->getModelMode()==0) {  modelMode="Nominal";     } else {    modelMode="True";    }

    QString operatorMode;
    if(cThread->mm3a->getOperatorMode()==0) {   operatorMode="Blind";   } else {    operatorMode="Redirecting";     }

    QString inputDevice;
    if(cThread->spaceExp==1) {  inputDevice="Space explorer";    } else {    inputDevice="Phantom Omni";     }

    QString motionPlane;
    if(cThread->mm3a->expPlane==0) {    motionPlane="XY plane";     } else {    motionPlane="XZ plane";}

    msgBox.setText(QString("Configuration:\n Control Mode:  %1\n Model Mode:  %2\n Operator Mode:  %3 \n Input device:  %4\n Motion plane:  %5").arg(controlMode).arg(modelMode).arg(operatorMode).arg(inputDevice).arg(motionPlane));
    msgBox.setInformativeText("Do you want to proceed?");
    msgBox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel);
    msgBox.setDefaultButton(QMessageBox::Ok);
    int ret = msgBox.exec();

    switch (ret) {
    case QMessageBox::Ok:
        // start experiment after confirmation from user

        // check state of record button
        cam->rec(recState->checkState());

        // start control thread
        qDebug() << __LINE__ << Q_FUNC_INFO << ": Starting control thread";
        cThread->open();
        cThread->start();


        // start joysticks only if control mode is free.
        if(cThread->mode==1) {
            // check which joystick is to be used
            if(joystickBox->isChecked()) {
                cThread->spaceExp = 1;
                cThread->omni = 0;
                joystick->open();
                joystickTimer->start();




            } else {
                cThread->spaceExp = 0;
                cThread->omni = 1;
                omni->open();
                omni->start();
                omniTimer->start();
            }
        }
        break;

    case QMessageBox::Cancel:
        break;

        qDebug() << __LINE__ << Q_FUNC_INFO;
    }
}

// stop control thread
void nanoCalib::stopExp() {
    qDebug()<<"nanoCalib::stopExp(): Stopping control thread";
    cThread->close();

    if(omniTimer->isActive()) {
        omniTimer->stop();
        omni->close();
    } else {
        if(joystickTimer->isActive()) {
            joystickTimer->stop();
        }
    }
}


void nanoCalib::keyPressEvent(QKeyEvent *event)
{
    if(event->key() == Qt::Key_Escape) {
        qDebug()<< "Escape Pressed";
        qApp->quit();
    }

    if(event->key() == Qt::Key_Alt)
        cThread->mm3a->recordPoints();
}

void nanoCalib::setMode()
{

    if(freeMode->isChecked()) {
        cThread->mode = 1;
    } else {
        cThread->mode = 0;
    }
    qDebug()<<"MainWindow::setMode(): "<<cThread->mode;
}

void nanoCalib::setExpPlane()
{

    if(xyPlane->isChecked())
        cThread->mm3a->expPlane = 0;
    else
        cThread->mm3a->expPlane = 1;

    qDebug() << "MainWindow::setExpPlane(): "<< cThread->mm3a->expPlane;
}



void nanoCalib::setBlobDetect(int value)
{
    cam->blob_detect = value;
    qDebug() << value;
}
